/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-11 13:52:35
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-13 11:26:11
 * @Description: ...
 * @Reference: ...
 */

#include "fog_converter.h"

namespace kaist_publisher {

FogConverter::FogConverter(ros::NodeHandle &nh, const std::string &dataset_dir,
                           const std::string &save_dir,
                           const std::string &topic)
    : Converter(dataset_dir, save_dir), topic_(topic) {
    bag_name_ = FilterSlash(topic_) + ".bag";
    pub_fog_ = nh.advertise<irp_sen_msgs::fog_3axis>(topic_.c_str(), 40);
}

int64_t FogConverter::Init_Publish() {
    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    fp_ = fopen(data_file.c_str(), "r");

    if (fscanf(fp_, "%ld,%f,%f,%f\n", &stamp_, &roll_, &pitch_, &yaw_) == 4) {
        return stamp_;
    } else {
        fclose(fp_);
        return LONG_MAX;
    }
}

int64_t FogConverter::Publish() {
    irp_sen_msgs::fog_3axis fog_data;
    fog_data.header.stamp.fromNSec(stamp_);
    fog_data.header.frame_id = "dsp1760";
    fog_data.d_roll = roll_;
    fog_data.d_pitch = pitch_;
    fog_data.d_yaw = yaw_;

    pub_fog_.publish(fog_data);

    if (fscanf(fp_, "%ld,%f,%f,%f\n", &stamp_, &roll_, &pitch_, &yaw_) == 4) {
        return stamp_;
    }

    fclose(fp_);
    return LONG_MAX;
}

void FogConverter::Publish(int64_t &timestamp,
                           irp_sen_msgs::fog_3axisPtr &fog_data) {
    fog_data.reset(new irp_sen_msgs::fog_3axis());
    fog_data->header.stamp.fromNSec(stamp_);
    fog_data->header.frame_id = "dsp1760";
    fog_data->d_roll = roll_;
    fog_data->d_pitch = pitch_;
    fog_data->d_yaw = yaw_;

    // pub_fog_.publish(fog_data);

    if (fscanf(fp_, "%ld,%f,%f,%f\n", &stamp_, &roll_, &pitch_, &yaw_) == 4) {
        timestamp = stamp_;
        return;
    }

    fclose(fp_);
    timestamp = LONG_MAX;
    return;
}

int FogConverter::Convert() {
    CheckAndCreateSaveDir();

    boost::filesystem::path bag_file =
        boost::filesystem::path(save_dir_) / bag_name_;
    // rosbag::Bag bag(bag_file.string(), rosbag::bagmode::Write);
    // ROS_INFO("saving %s", bag_file.c_str());

    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    FILE *fp = fopen(data_file.c_str(), "r");
    irp_sen_msgs::fog_3axis fog_data;
    int64_t stamp;
    float roll, pitch, yaw;
    while (fscanf(fp, "%ld,%f,%f,%f\n", &stamp, &roll, &pitch, &yaw) == 4) {
        fog_data.header.stamp.fromNSec(stamp);
        fog_data.header.frame_id = "dsp1760";
        fog_data.d_roll = roll;
        fog_data.d_pitch = pitch;
        fog_data.d_yaw = yaw;

        // bag.write(topic_, fog_data.header.stamp, fog_data);
        pub_fog_.publish(fog_data);
    }
    // bag.close();
    fclose(fp);
    // ROS_INFO("done saving %s", bag_file.c_str());

    return 0;
}

}  // namespace kaist_publisher